#ifndef _unskew_H_
#define _unskew_H_

#include <algorithm>

#include <vector>

#include "NearestMeanClassifier.h"
#include "Sample.h"
#include "DataSet.h"
#include "filter.h"
#include "Image.H"
#include "IO.H"
#include "Util.H"
#include "graymorphology.h"
#include "unskew.h"
//#include "redlesiondetection.h"


using namespace std;
using namespace pip;

template<typename T>
inline T min(T &a,T &b){
	return (a > b)?b:a;
}


template<typename T>
double mse(Image<T> &im,const double &n, double init=14){
	double MSE(0),se(0),fx(init),fy(init),bx(init),by(init),dist(0);
	for(int x=0; x < im.dim(0); ++x){
		for(int y=0; y < im.dim(1); ++y){
			if(im(x,y)>0){
				fx=fy=bx=by=init;
				dist=999999;
				while(true){
						se=0;
						--bx;
						++fx;
						fy+=n;
						by-=n;
						if(   bx <  0 || 
							  by <  0 ||
							  by >= 28||
							  fx >= 28||
							  fy >= 28||
							  fy < 0){
								  break;
						}
						se=min(((x-fx)*(x-fx)+(y-fy)*(y-fy)),((x-bx)*(x-bx)+(y-by)*(y-by)));
						if(se < dist)
							dist=se;
				}
				if(dist < 392)
					MSE+=dist;
				else
					MSE+=392;
			}
		}
	}
	//cout << count << " " << MSE << endl;
	return MSE;
}

template<typename T>
double best_slope(Image<T> &im, int border, double steps){
	double curr_mse(9999999),min_mse(9999999),best_slope(0);
	for(double s=-border; s <= border; s+=1/steps){
		curr_mse=mse(im,s);
		//cout << s << "  \t\t?\t\t "<< curr_mse << endl;
		if( curr_mse < min_mse){
			min_mse=curr_mse;
			best_slope=s;
			//cout << "\n\t\t best:" <<best_slope << " " << min_mse <<" "<< endl;
		}
	}
	//cout << best_slope << "FINAL"<<endl;
	if( abs(best_slope) > 0.01 && 
		abs(best_slope) < 13)
		best_slope=-1/best_slope*1/2;
	else
		best_slope=0;
	return best_slope;
}




template<typename T>
void unskew(Image<T> &im,int stop=0){
	Image<T> tmp;
	int halt(im.dim(2)),width(im.dim(0));
	double dist(0),slope(0);
	if (halt > 1){
		cout << "\n=>unskewing... [mse]"<<endl;
		if(stop)
			halt=stop;
		for(int i=0; i < halt;++i){
			if(!(i%25))
				cout<<"|";
			tmp=im.sliced(2,i);
			//medianFilterFast(tmp,3); 
			slope=best_slope(tmp,13,2);
			//cout << "best_slope: "<< slope <<endl;
			for(int y = 0; y < width; ++y) {
				//cout<< y << " " << int((13-y)*slope) << " " << int((y%14)*slope) << endl;
				for(int x = 0; x < width; ++x) {
					if(y<14){
						dist=x+(13-y)*slope;
						if( 0 <= dist && dist < width){
							im(x,y,i)=tmp(int(dist),y);
						}else{
							im(x,y,i)=0;
						}
					}else{
						dist=x+(-13+y)*-slope;
						if( 0 <= dist && dist < width){
							im(x,y,i)=tmp(int(dist),y);
						}else{
							im(x,y,i)=0;
						}
					}
				}
			}
		}
		cout << endl;
	}else{
		tmp=im;
		slope=best_slope(tmp,13,2);
		//cout << "best_slope: "<< slope <<endl;
		for(int y = 0; y < width; ++y) {
			//cout<< y << " " << int((13-y)*slope) << " " << int((y%14)*slope) << endl;
			for(int x = 0; x < width; ++x) {
				if(y<14){
					dist=x+(13-y)*slope;
					if( 0 <= dist && dist < width){
						im(x,y)=tmp(int(dist),y);
					}else{
						im(x,y)=0;
					}
				}else{
					dist=x+(-13+y)*-slope;
					if( 0 <= dist && dist < width){
						im(x,y)=tmp(int(dist),y);
					}else{
						im(x,y)=0;
					}
				}
			}
		}
	}
}

			

template<typename T>
void unskewLOL(Image<T> &im,int stop=0){
	Image<T> tmp;
	int halt(im.dim(2)),width(im.dim(0));
	double offset(0);
	int number(0);
	if(halt > 1){
		cout << "\n=>unskewing...[linewise]"<<endl;
		if(stop)
			halt=stop;
		for(int i=0; i < halt;++i){
			if(!(i%50))
				cout<<"|";
			tmp=im.sliced(2,i);
			for(int y = 0; y < width; ++y) {
				offset=0;
				number=0;
				for(int x = 0; x < width; ++x) {
					if(tmp(x,y)>0){
					   offset+=x;
					   number++;
					}
				}
				offset/=number;
				offset=-13+offset;
				for(int x = 0; x < width; ++x) {
					if(    int(x+offset)>=0 
						&& int(x+offset)< 28
						&& tmp(int(x+offset),y) > 220){
							//cout<< int(offset) << " " << x << "offset plus x: " << int(x+offset) << endl;
					   im(x,y,i)=255;
					}else{
						im(x,y,i)=0;
					}
				}
			}
			for(int x = 0; x < width; ++x) {
				offset=0;
				number=0;
				for(int y = 0; y < width; ++y) {
					if(tmp(x,y)>0){
					   offset+=y;
					   number++;
					}
				}
				offset/=number;
				offset=-13+offset;
				for(int y = 0; y < width; ++y) {
					if(    int(y+offset)>=0 
						&& int(y+offset)< 28
						&& tmp(x,int(y+offset)) > 220){
							//cout<< int(offset) << " " << x << "offset plus x: " << int(x+offset) << endl;
					   im(x,y,i)=255;
					}else{
						im(x,y,i)=0;
					}
				}
			}
		}
	cout << endl;
	}else{
			tmp=im;
			for(int y = 0; y < width; ++y) {
				offset=0;
				number=0;
				for(int x = 0; x < width; ++x) {
					if(tmp(x,y)>220){
					   offset+=x;
					   number++;
					}
				}
				offset/=number;
				offset=-13+offset;
				for(int x = 0; x < width; ++x) {
					if(    int(x+offset)>=0 
						&& int(x+offset)< 28
						&& tmp(int(x+offset),y) > 220){
							//cout<< int(offset) << " " << x << "offset plus x: " << int(x+offset) << endl;
					   im(x,y)=255;
					}else{
						im(x,y)=0;
					}
				}
			}
			for(int x = 0; x < width; ++x) {
				offset=0;
				number=0;
				for(int y = 0; y < width; ++y) {
					if(tmp(x,y)>220){
					   offset+=y;
					   number++;
					}
				}
				offset/=number;
				offset=-13+offset;
				for(int y = 0; y < width; ++y) {
					if(    int(y+offset)>=0 
						&& int(y+offset)< 28
						&& tmp(x,int(y+offset)) > 220){
							//cout<< int(offset) << " " << x << "offset plus x: " << int(x+offset) << endl;
					   im(x,y)=255;
					}else{
						im(x,y)=0;
					}
				}
			}
	}

}


		//}
		/*
			dx=0;
			dy=0;	
			fx=14;
			fy=14;
			bx=14;
			by=14;
			dist=614657;
			while( true ){
					--bx;
					++fx;
					fy+=a;
					by-=a;
					if(!(fx <  28 
					&& fy <  28 
					&& fy >= 0 
					&& by >= 0
					&& by <  28
					&& bx >= 0)){
						 break;
					}
					data(fx,fy,i)=255;
					data(bx,by,i)=255;
			}*/

void medianFilterFast(Image <unsigned char> &image, const int &n)
{//(r):sonka & hugo_kruift
  // Initialize variables
  int dx, dy, x, y, kernelCentre(n / 2), nm, initialNm, t(n * n / 2);
  Image<unsigned char> tempImage(image.bordered(kernelCentre, border_mirror));
  unsigned char m, initialM, pg;

  // Create initial histogram at (0,0) and compute the median
  int histogram[256], initialHistogram[256] = {0};

  //   Fill histogram
  for (dy = -kernelCentre; dy <= kernelCentre; ++dy) {
    for (dx = -kernelCentre; dx <= kernelCentre; ++dx) {
      initialHistogram[tempImage(dx, dy)]++;
    }
  }

  //   Compute initial mean (m) and number of pixels before the mean (nm)
  for (initialM = 0, initialNm = initialHistogram[initialM]; initialNm <= t; initialNm += initialHistogram[++initialM]);

  // Process whole image
  for (y = 0; y < image.dim(1); ++y) {
    // Copy the initial histogram, mean and nm for this row to the current histogram, mean and nm
    memcpy(histogram, initialHistogram, 256 * sizeof(int));
    m = initialM; nm = initialNm;

    // Shift histogram down. Only not at the first row, then we already have the correct histogram.
    if (y != 0) {
      // Create new initial histogram for this row, based on the initial histogram of the previous row
      for (dx = -kernelCentre; dx <= kernelCentre; ++dx) {
        // Clear top column
        pg = tempImage(dx, (y - 1) - kernelCentre);
        histogram[pg]--;
        if (pg <= m) --nm;

        // Fill bottom column
        pg = tempImage(dx, y + kernelCentre);
        histogram[pg]++;
        if (pg <= m) ++nm;
      }

      // Find the median. The median is at the location where the cumulative histogram (stored in nm) is equal to or larger than t.
      for (; nm <= t; ++m, nm += histogram[m]);
      for (; nm - histogram[m] > t; nm -= histogram[m], --m);

      // Copy the current histogram for this row, so the next row can use it as the initial histogram
      memcpy(initialHistogram, histogram, 256 * sizeof(int));
      initialM = m; initialNm = nm;
    }

    // Set the median
    image(0, y) = m;

    for (x = 1; x < image.dim(0); ++x) {
      for (dy = -kernelCentre; dy <= kernelCentre; ++dy) {
        // Clear left column
        pg = tempImage((x - 1) - kernelCentre, y + dy);
        histogram[pg]--;
        if (pg <= m) --nm;

        // Fill right column
        pg = tempImage(x + kernelCentre, y + dy);
        histogram[pg]++;
        if (pg <= m) ++nm;
      }

      // Find the median. The median is at the location where the cumulative histogram (stored in nm) is equal to t.
      for (; nm <= t; ++m, nm += histogram[m]);
      for (; nm - histogram[m] > t; nm -= histogram[m], --m);

      // Set the median
      image(x, y) = m;
    }
  }
}

#endif